#pragma once

#include "VisioCommon.h"
#include <opencv2\opencv.hpp>
#include "Meshes.h"
#include "MeshInstances.h"
#include "ImageData.h"
#include "math_3d.h"

#define POLY_INVERSE_DEGREE 6

class CCamera;

typedef vector<CCamera*> CamerasVector;
typedef std::pair<CCamera*, CCamera*> CameraPair;

struct SCameraParams
{
	SCameraParams();
	SCameraParams(cv::Mat _R, cv::Mat _t, cv::Mat _K, cv::Mat _P);
	cv::Mat R;
	cv::Mat t;
	cv::Mat K;
	cv::Mat P;
};

struct SCameraConstriants
{
	char bConstrianed[9];
	double Constraints[9];
	double Weights[9];
};

class CCamera
{
public:
	CCamera(CCameraMesh* pMesh);
	CCamera(CCameraMesh* pMesh, SCameraParams params, CImageData* pImageData);
	CCamera(CCameraMesh* pMesh, cv::Mat rotationMat, cv::Mat translationVector, cv::Mat Image, float f);
	void SetExtrinsicParameters(cv::Mat rotationMat, cv::Mat translationVector);
	CCameraMeshInstance* CCamera::CreateMeshInstance();
	SCameraParams GetParameters() const;
	double* GetRodirougsRotationAsArray() const;
	double GetFocal() const;
	void Update(cv::Mat R, cv::Mat t, double* IntrinParams);
	void PrintCamera() const;
	cv::Point2d Project(double* _p, bool bUseInstrinsic, bool bUndistort) const;
	cv::Point2d Project(cv::Point3d _p, bool bUseInstrinsic, bool bUndistort) const;
	cv::Point2d GetKeyPoint(UINT index) const;
	UINT GetImageIndex() const;
	CTrackData* GetTrackFromKeyPointIndex(UINT KeyPointIndex) const;
	CImageData* GetImageData() const;
	cv::Mat GetCameraCenter() const;
	void UseDefaulFocalLength();
	UINT GetAddedIndex() const;
	void MarkPoint(UINT KeyPointIndex, int mark);
	UINT GetNumVisablePoints() const;
	UINT GetNumPointsProjected() const;
	void RemoveMark(UINT KeyPointIndex);
	double GetDistorationParams( UINT index) const;
	const double* GetDistorationParams() const;
	void UpdateFocalPoint(double Focal);
	void UpdateIntrin(double Focal, double* Distoration);
	cv::Mat UndistortNormalizedPoint(cv::Mat p) const;
	SCameraConstriants GetConstraints() const;
	void ResetDistortationPoly();
	void PrintCameraToFile(FILE* pFile) const;
	KeyColor GetColorOfKeyPoint(UINT index) const;
private:

	void InitExtrinsicParameters();
	void InitConstraints();
	void UpdateInvDistoration(double k1, double k2, double Focal);
	UINT GetImageHeight() const;
	UINT GetImageWidth() const;
	





	cvImage m_Image;
	cv::Mat m_RotationMatrix;
	cv::Mat m_TranslationMatrix;
	cv::Mat m_CameraCenterVector;
	CCameraMesh* m_pMesh;
	CCameraMeshInstance* m_pInst;
	SCameraParams m_Params;
	SCameraConstriants m_Constraints;
	UINT m_Index;
	CImageData* m_pImageData;
	double m_Focal;
	double m_Distoration[2];
	double m_InvDistorationPolynomial[POLY_INVERSE_DEGREE];
};
